/*************************************************************************
*
*	simulate_fluorometer.cpp
*
*  Copyright 2007,2008,2009 Steve Stancliff
*
*  Part of the TAOSF SSA system.
*
*
*  This module provides functions for simulating fluorometer readings
*
*
**************************************************************************
*
*  version 1.0.3, 4/24/09, SBS
*   - documentation
*
*  version 1.0.2, 10/4/08, SBS
*   - documentation
*
*  version 1.0.1, 8/27/08, SBS
*   - add multiple blooms
*   - add random bloom locations
*   - pull Gaussian() function out of simulate_FluorometerGaussian()
*   - use status_SetTimeString()
*   - merge fluorometerGaussian() and fluorometer()
*
*  version 1.0, 5/17/08, SBS
*    - split from simulate_science.cpp
*
*
**************************************************************************
*
*  This program is free software; you can redistribute it and/or modify
*  it under the terms of the GNU General Public License as published by
*  the Free Software Foundation; either version 2 of the License, or
*  (at your option) any later version.
*
*  This program is distributed in the hope that it will be useful, but 
*  WITHOUT ANY WARRANTY; without even the implied warranty of 
*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 
*  General Public License for more details.
*
*  You should have received a copy of the GNU General Public License 
*  along with this program; if not, write to the Free Software Foundation, 
*  Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 
*
**************************************************************************/
/**
*  @file 
*  @ingroup plat_sci_simulate
*  @brief
*  This module provides functions for simulating fluorometer readings
*  <hr>
**/


#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include <cmath>//pow
#include <vector>
#include <utility>//pair

#include "string.h"

#include "waypoints.h"
#include "platform_types.h"
#include "platform_modes.h"
#include "status.h"
//#include "logfile.h"
#include "conversions.h"

#include "simulate_fluorometer.h"

#include "../simulator.h"



// holds the center for each bloom
static POSITION bloomcenter[NUMBLOOMS];

// holds the relative size for each bloom
static POSITION bloomsize[NUMBLOOMS];

using namespace std;


/*
struct UTMdatum {

  UTMdatum() {
    northing = -1;
    easting = -1;
    zone = -1;
  }

  double northing;
  double easting;
  double zone;

};
*/

/********************************************************
*
* convertFromLatLon()
*
* Given current location in decimal latitude longitude,
*    calculate UTM northing, easting, and zone.
*    Conversions obtained from: http://www.uwgb.edu/dutchs/UsefulData/UTMFormulas.HTM
*
********************************************************/
/*
UTMdatum convertFromLatLon(double latDeci, double lonDeci){  
  double latitude = latDeci * M_PI/180; //in radians
  double longitude = lonDeci * M_PI/180; //in radians

  double a = 6378137; //equitorial radius in meters (NAD83/WGS84)
  double b = 6356752.3142; //polar radius in meters (NAD83/WGS84)
  //double flattening = 1/298.257223563; //(a-b)/a (NAD83/WGS84)
  double zone= -1;
  if (longitude < 0) {
    zone = floor((180+lonDeci)/6)+1;
  }
  else {
    zone = floor(lonDeci/6) + 31;
  }
  double zoneCM = 6*zone-183;
  if(zone != 18) {
    cout<<"!!not in Chesapeake Bay "<<latDeci<<" "<<lonDeci<<endl;    
  }
  double long0 = zoneCM; //central meridian of zone
  double k0 = 0.9996; //scale along long0
  double eccentricity = sqrt(1-b*b / (a*a)); //approximately 0.08 
  double e1squared = pow(eccentricity,2) / (1 - pow(eccentricity,2));//approximately 0.007
  double n = (a-b)/(a+b);
  double denom = 1-pow(eccentricity*sin(latitude),2);
  //double rho = a*(1-pow(eccentricity,2))/pow(denom,1.5);
  double nu = a / pow(denom,0.5);
  double p = (lonDeci - long0) * 3600/10000; //convert to sec
  double sin1 = M_PI/(180*60*60);
  //compute the meridonal arc (army form)
  double bigA = a * (1 - n + (5 * pow(n,2)/4) * (1-n) + (81 * pow(n,4)/64) *(1-n));
  double bigB = (3*a*n/2) * (1- n - (7 * pow(n,2)/8) * (1-n) + (55 * pow(n,4)/64));
  double bigC = (15*a*pow(n,2)/16) * (1 - n + (3 * pow(n,2)/4)*(1-n));
  double bigD = (35*a*pow(n,3)/48) * (1 - n + (11 * pow(n,2)/16));
  double bigE = (315*a*pow(n,4)/51)*(1-n);
  double bigS = bigA*latitude - bigB*sin(2*latitude) + bigC*sin(4*latitude) - bigD*sin(6*latitude + bigE*sin(8*latitude));
  //compute the meridonal arc (USGS form)
  // double firstTerm = (1 - pow(eccentricity,2)/4 
  // 		      - 3 * pow(eccentricity,4)/64 
  // 		      - 5 * pow(eccentricity,6)/256)*latitude;
  //   double secondTerm = (3 * pow(eccentricity,2)/8 
  // 		       + 3* pow(eccentricity,4)/32 
  // 		       + 45*pow(eccentricity,6)/1024)*sin(2*latitude);
  //   double thirdTerm =  (15*pow(eccentricity,4)/256 
  // 		       + 45*pow(eccentricity,6)/1024) * sin(4*latitude);
  //   double fourthTerm = 35*pow(eccentricity,6)/3072*sin(6*latitude);
  //double bigM = a*(firstTerm - secondTerm + thirdTerm - fourthTerm);
  //determine which meridional term to use
  double meridionalArc = bigS;
  //compute terms for y=northing
  double k1 = meridionalArc * k0;
  double k2 = k0 * pow(sin1,2) * nu * sin(latitude)*cos(latitude) * (1e8)/2;
  double k3 = k0*pow(sin1,4)*nu*sin(latitude)*pow(cos(latitude),3)/24 
    * (5 - pow(tan(latitude),2) + 9 * e1squared*pow(cos(latitude),2) 
       + 4 * pow(e1squared,2)*pow(cos(latitude),4)) * (1e16);
  double northing = k1 + k2*pow(p,2) + k3*pow(p,4);
  //compute terms for x=easting
  double k4 = k0 * sin1 * nu * cos(latitude) * (1e4);
  double k5 = k0 * pow(sin1,3) * nu * pow(cos(latitude),3)/6 
    * (1 - pow(tan(latitude),2) + e1squared * pow(cos(latitude),2)) * (1e12);
  double easting = k4 * p + k5 * pow(p,3); //relative to central meridian
  easting = 500000 + easting;

  UTMdatum myDatum;
  myDatum.zone = zone;
  myDatum.northing = northing;
  myDatum.easting = easting;
  return myDatum;
}
*/

/*
struct FLUOROMETER_STRUCT{
  double NWLonDeg; //in degrees
  double NWLatDeg;
  double SELonDeg;
  double SELatDeg;
  double resolution; //in meters

  //parameters for parallel strips
  double stripNWLonDeg; //in degrees
  double stripNWLatDeg;
  int numStrips;
  double stripLength; //in meters
  double stripThickness; //in meters
  double stripSeparation; //in meters


  UTMdatum northwestUTM; //in UTM coords
  UTMdatum southeastUTM; //in UTM coords
  UTMdatum stripNWcornerUTM; //in UTM coords
  int numEastingCells;
  int numNorthingCells;
  int numStripEastingCells;
  int numStripNorthingCells;
  int numStripSeparationCells;  

  //this holds the contents of the fluorometer readings
  vector<vector<double> > fluorometerVals;
  
  // ********************************************************
  // *
  // * UTMToIndices()
  // *
  // * Find the index into the flurometerVals vector<vector>
  // * from UTM northing easting
  // *
  // ********************************************************
  pair<int,int> UTMToIndices(UTMdatum & myUTM) {
    int rowNdx = static_cast<int> (round((northwestUTM.northing - myUTM.northing)/resolution));
    int colNdx = static_cast<int> (round((myUTM.easting - northwestUTM.easting)/resolution));
    return make_pair(rowNdx,colNdx);
  }

  // ********************************************************
  // *
  // * degreesToIndices()
  // *
  // * Find the index into the flurometerVals vector<vector>
  // * from Latitude Longitude decimal degrees
  // *
  // ********************************************************
  pair<int,int> degreesToIndices(double latDeg, double lonDeg) {
    UTMdatum myUTM = convertFromLatLon(latDeg,lonDeg);
    return UTMToIndices(myUTM);  
  }

  // ********************************************************
  // *
  // * ObtainFluorometerReading()
  // *
  // * Obtain the fluorometer value at the latitude longitude
  // *
  // ********************************************************
  double ObtainFluorometerReading(double latDeci,double lonDeci) {
    pair<int,int> myIndices;
    myIndices = degreesToIndices(latDeci, lonDeci);
    //Check that the query is within pre-defined fluorometer reading area
    if(myIndices.first < 0 | myIndices.first >= numNorthingCells |
       myIndices.second < 0 | myIndices.second >= numEastingCells) {
      //cout<<"("<<latDeci<<", "<<lonDeci<<") is outside of defined fluorometer reading area"<<endl;
      return -1;
    }
    return fluorometerVals[myIndices.first][myIndices.second];
  }
  
};
*/

/********************************************************
*
* InitFluorometer()
*
* Set initial values for fluorometer status
*
********************************************************/
/*
static FLUOROMETER_STRUCT InitFluorometer() {

  FLUOROMETER_STRUCT myStruct;
  //set the northwest and southeast corner of all latitude longitudes to query
  myStruct.NWLonDeg = SIMBOXWEST;
  myStruct.NWLatDeg = SIMBOXNORTH;
  myStruct.SELonDeg = SIMBOXEAST;
  myStruct.SELatDeg = SIMBOXSOUTH;
  
//  myStruct.resolution = 5;//resolution of cells in meters
  myStruct.resolution = CELLSIZE;//resolution of cells in meters

  //set the number of dye strips
  myStruct.numStrips = 2;
  //set length of dye strips in meters
  myStruct.stripLength = 150;
  //set thickness of dye strips in meters
  myStruct.stripThickness = 5;
  //set distance separating borders of neighboring strips in meters
  myStruct.stripSeparation = 30;

  //set the northwest corner of the dye patch
  //this particular point is 2m east, 5m south of 38.05N 75.05W degrees
//  myStruct.stripNWLonDeg = -75.04997717414854;
//  myStruct.stripNWLatDeg = 38.04995494559304;
  myStruct.stripNWLonDeg = INIT_LON;
  myStruct.stripNWLatDeg = INIT_LAT;



  //convert Latitude and Longitude to UTM coords
  myStruct.northwestUTM =  convertFromLatLon(myStruct.NWLatDeg,myStruct.NWLonDeg);
  myStruct.southeastUTM =  convertFromLatLon(myStruct.SELatDeg,myStruct.SELonDeg);
  myStruct.stripNWcornerUTM = convertFromLatLon(myStruct.stripNWLatDeg,myStruct.stripNWLonDeg);

  //compute the number of cells in the grid
  myStruct.numEastingCells = static_cast<int> (ceil(myStruct.southeastUTM.easting) 
					       - floor(myStruct.northwestUTM.easting));
  
  myStruct.numNorthingCells = static_cast<int> (ceil(myStruct.northwestUTM.northing) 
						- floor(myStruct.southeastUTM.northing));
  //check that total number of cells fits in memory!
  double tmpNumCells = myStruct.numEastingCells / myStruct.resolution 
    * myStruct.numNorthingCells / myStruct.resolution;
	 
  if(tmpNumCells > MAXCELLS) {
    myStruct.resolution = myStruct.resolution * ceil (tmpNumCells / MAXCELLS);
    cout<<"Flurometer reading  area is too large for specified resolution."<<
      "Resetting resolution to "<<myStruct.resolution<<" meters."<<endl;
  }

  myStruct.numEastingCells = static_cast<int> (myStruct.numEastingCells/myStruct.resolution);
  myStruct.numNorthingCells = static_cast<int> (myStruct.numNorthingCells/myStruct.resolution);  
  myStruct.numStripEastingCells = static_cast<int> (round(myStruct.stripLength
							  /myStruct.resolution));
  myStruct.numStripNorthingCells = static_cast<int> (round(myStruct.stripThickness
							   /myStruct.resolution));
  myStruct.numStripSeparationCells = static_cast<int>(round(myStruct.stripSeparation
							    /myStruct.resolution));
  
  //store fluorometry data in vector of vectors
  vector<double> eastVector(myStruct.numEastingCells,0);
  myStruct.fluorometerVals.resize(myStruct.numNorthingCells,eastVector);

  //find the indices into the fluorometry vector of vectors for northwest strip corner
  pair<int, int> stripNWIndices = myStruct.UTMToIndices(myStruct.stripNWcornerUTM);
  int nCellNdx;
  int eCellNdx;

  //check that the strips fall within the NW and SE boundaries
  if (stripNWIndices.first < 0 | 
      stripNWIndices.first + myStruct.numStripNorthingCells > myStruct.numNorthingCells | 
      stripNWIndices.second < 0 | 
      stripNWIndices.second + myStruct.numStripEastingCells > myStruct.numEastingCells) {
    cout<<"Fluorometer strips fall outside of defined NW and SE boundaries."<<endl;
    cout<<"Resetting to 0 fluorometer strips."<<endl;
    myStruct.numStrips = 0;    
  }

  double highFluorometryValue = 1;

  //lay down the strips by setting cells of the fluorometry vector to the high fluorometry value
  for(int whichStrip = 0; whichStrip < myStruct.numStrips; ++whichStrip) {
    //iterate through the strips
    for(int whichNCell = 0; whichNCell < myStruct.numStripNorthingCells; ++whichNCell) {
	nCellNdx = stripNWIndices.first + whichNCell 
	  + whichStrip*(myStruct.numStripSeparationCells+myStruct.numStripNorthingCells);
      //iterate through northing cells
      for(int whichECell=0; whichECell < myStruct.numStripEastingCells; ++whichECell) {
	//iterate through easting cells
	eCellNdx = stripNWIndices.second+whichECell;
	myStruct.fluorometerVals[nCellNdx][eCellNdx] = highFluorometryValue;
      } //end for int whichECell
    }//end for int whichNCell
  }//end for int whichStrip
  
  return myStruct;  
}
*/


/**
***************************************************************************
*
* Gaussian()
*/
/** 
*
* @brief
* Return value of a gaussian distribution at a point
*
*
********************************************************/
static double Gaussian(
			POSITION p
			,POSITION center
			,POSITION stdev
			,double corr)
{
	double rval;
	
	rval = exp(-1.0/(2.0*(1.0-SQ(corr))) * 
			(SQ((p.dLatDeg - center.dLatDeg ) / stdev.dLatDeg ) 
			 + SQ( (p.dLonDeg - center.dLonDeg ) / stdev.dLonDeg)
			 - 2.0*corr*(p.dLatDeg - center.dLatDeg ) *
			 (p.dLonDeg - center.dLonDeg ) / (stdev.dLatDeg*stdev.dLonDeg)));

	return(rval);
}


/**
***************************************************************************
*
* simulate_Fluorometer()
*/
/** 
*
* @brief
* makes an oval bloom 
*
********************************************************/
extern "C" SCI_DBL simulate_Fluorometer(
			POSITION p
			,time_t t)
{
	SCI_DBL sd = status_InitSciDbl();
	tm *timeinfo;
	register int i;
		
	sd.dValue = 0;
	
//	printf("hello from simulate_Fluorometer()\n");
	
	for(i=0;i<NUMBLOOMS;i++){

//		printf("creating bloom #%d\n",i);
		
		if(i<NUMROUNDBLOOMS){
			#ifdef BLOOM_GAUSSIAN	// gaussian-distributed values
	
			sd.dValue += Gaussian(p,bloomcenter[i],bloomsize[i],CORR_LAT_LON);
	
			#else	// constant values
	
			if (( SQ( (p.dLatDeg - bloomcenter[i].dLatDeg ) / bloomsize[i].dLatDeg ) 
						+ SQ( (p.dLonDeg - bloomcenter[i].dLonDeg ) / bloomsize[i].dLonDeg)) < 1){
						
				sd.dValue = 1.;
			}
	
			#endif // BLOOM_GAUSSIAN
		}else{	// stripes
			register double m,n;
			m = p.dLatDeg - bloomcenter[i].dLatDeg;
			n = p.dLonDeg - bloomcenter[i].dLonDeg;
			
			if (( SQ( (m ) / bloomsize[i].dLatDeg ) 
					+ SQ( (n ) / bloomsize[i].dLonDeg)) < 1){

				if((fabs(m) > METERSTOLATDEG(STRIPESEPARATIONMETERS/2.))
							&&(fabs(m) < METERSTOLATDEG(STRIPEWIDTHMETERS + STRIPESEPARATIONMETERS/2.))){

					sd.dValue = 1.;
				}
			}
		
		}
//		printf("done\n");

	}
	
	if (sd.dValue < FLUOR_FLOOR) {
	  sd.dValue = 0.;
	}

	strncpy(sd.sUnits,"V", UNITSTRLEN);

	sd.position = p;

	timeinfo = localtime(&t);
//	strftime(sd.sTime, sizeof(sd.sTime), TIMEFORMAT, timeinfo);

	status_SetTimeString(sd.sTime, sizeof(sd.sTime),timeinfo);

	return(sd);
}



/**
***************************************************************************
*
* Fluorometer_Init()
*/
/** 
*
* @brief
*
*
********************************************************/
extern "C" void Fluorometer_Init()
{
	register int i,j;
	register double m,n;
	
	printf("hello from Fluorometer_Init\n");
	printf("NUMBLOOMS = %d\n",NUMBLOOMS);
	for(i=0;i<NUMBLOOMS;i++){

	printf("bloom #%d\n",i);
	
	//******************
	// set bloom centers
	//******************
		
		#ifdef RANDOMBLOOMS

		m = -0.5 + rand()/(((double)RAND_MAX + 1.) / 1.0);	// random number between -0.5 and 0.5
		n = -0.5 + rand()/(((double)RAND_MAX + 1.) / 1.0);	
	
		bloomcenter[i].dLatDeg = BLOOMCENTER_LAT  + BLOOMBOXSIZE_LAT *m ;
		bloomcenter[i].dLonDeg = BLOOMCENTER_LON  + BLOOMBOXSIZE_LON *n ;
		
		for(j=0;j<i;j++){
			// if not enough separation between blooms, redo this one
		
			if(fabs(bloomcenter[i].dLatDeg - bloomcenter[j].dLatDeg) < BLOOMSTD_LAT *2){
				printf("too close to old center\n");
				waypoints_PositionPrintWithString("old: ",bloomcenter[j]);
				waypoints_PositionPrintWithString("new: ",bloomcenter[i]);
				printf("delta LAT = %f\n",BLOOMSTD_LAT*2);
				printf("delta LAT = %f\n",LATDEGTOMETERS(BLOOMSTD_LAT*2));
				i--;
				continue;
			}
			if(fabs(bloomcenter[i].dLonDeg - bloomcenter[j].dLonDeg) < BLOOMSTD_LON *2){
				printf("too close to old center\n");
				waypoints_PositionPrintWithString("old: ",bloomcenter[j]);
				waypoints_PositionPrintWithString("new: ",bloomcenter[i]);
				printf("delta LON = %f\n",BLOOMSTD_LON*2);
				printf("delta LON = %f\n",LONDEGTOMETERS(BLOOMSTD_LON*2));
				i--;
				continue;
			}
		}
			
		#else // fixed blooms
		
		j = (i+1)/2;
		if(!(i&1)){  // isodd(i)
			j *= -1;
		}  // j = 0,1,-1,2,-2,...
		
		bloomcenter[i].dLatDeg = BLOOMCENTER_LAT  + BLOOMCENTER_LAT_OFFSET *j ;
		bloomcenter[i].dLonDeg = BLOOMCENTER_LON  + BLOOMCENTER_LON_OFFSET *j ;

		m = -2.0 + rand()/(((double)RAND_MAX + 1.) / 4.0);	// random number between -2 and 2
		bloomcenter[i].dLatDeg += BOATCENTER_LAT_OFFSET/3. *m ;

		#endif	// RANDOMBLOOMS

	//******************
	// set bloom extents
	//******************
		#ifdef BLOOMSIZE_RANDOM
		
		m = 0.5 + rand()/(((double)RAND_MAX + 1.) / 0.5);	// random number between 0.5 and 1.0
		n = 0.5 + rand()/(((double)RAND_MAX + 1.) / 0.5);	
	
		#else	// fixed bloom size
		
		m = 1.0;
		n = 1.0;
	
		#endif	//BLOOMSIZE_RANDOM
		
		bloomsize[i].dLatDeg = BLOOMSTD_LAT * m;
		bloomsize[i].dLonDeg = BLOOMSTD_LON * n;
	}
	return;
}
